#include "device/HandleHelper.h"

static Config _comConf("etc/communication/handler.sys");

HandleHelper::HandleHelper()
{
    allow_handler = false;
    allow_send = true;
    InstallMessage();
}

HandleHelper::~HandleHelper()
{
    if(allow_handler){
        StopService();
    }
}



int HandleHelper::stopListen()
{
    if(allow_handler){
        StopService();
    }
    return HD_NO_ERROR;
}

void HandleHelper::sendToDevice(QByteArray data)
{
    if(comType == T_USB){
        USBHelper::GetInstance()->p()->Send(data.toStdString().c_str());
    }else{
        SerialHelper::GetInstance()->p()->SendBuff(data.toStdString().c_str());;
    }
}


void HandleHelper::ConnectDevice()
{
    if(!_workService.isNull()){
        std::cout<<"\t 启动串口接收循环!"<<std::endl;
        _workService.data()->detach();
    }else {
        qWarning("\t 手柄线程未指定!");
    }

}


/// 解码
void HandleHelper::DecodeData(std::string data)
{
    std::lock_guard<std::recursive_mutex> lck(_mtx) ;


    recvTime = std::chrono::steady_clock::now() ;

    /// RS232相对于USB有索引增量，USB模式需要去掉增量
    T_PROTO_I DEL = (comType == T_USB)? (T_PROTO_I)1:(T_PROTO_I)0;
    bool useUSB = (comType == T_USB)?  true:false;
    bool crcOK = true;
    if(!useUSB){
        // crcOK = (T_P_HEADER == (UC)data[0])  && CRCCheck(data);
        /// 中断模式已经进行过校验位检测了
        crcOK = (T_P_HEADER == (UC)data[0]) ;   /// 只需进行帧头检测就可以了
    }
    if( crcOK){
        /// 获取急停消息
        /// 这里需要取反一下
            _cur_Recv.criticalStop  = !GetButtonPressed((UC)data[T_I_B1-DEL],_verIndex._B_CRITICAL);
        /// 获取暂停/运行状态
            _cur_Recv.isPause       = GetButtonPressed((UC)data[T_I_B1-DEL],_verIndex._B_PAUSE);
        /// 获取使能状态
            _cur_Recv.enMotion      = GetButtonPressed((UC)data[T_I_B1-DEL],_verIndex._B_EN);
        /// 获取收丝状态
            _cur_Recv.inSilk        = GetButtonPressed((UC)data[T_I_B1-DEL],_verIndex._B_OUT);
        /// 获取送丝状态
            _cur_Recv.outSilk       = GetButtonPressed((UC)data[T_I_B1-DEL],_verIndex._B_IN);
        /// 获取运动类型
            _cur_Recv.motionType    = GetButtonPressed((UC)data[T_I_B1-DEL],_verIndex._B_MOTION);
        /// 获取坐标类型
            _cur_Recv.coordinate    = GetButtonPressed((UC)data[T_I_B1-DEL],_verIndex._B_COOR);
        /// Speed
            _cur_Recv.speed         = HLDataCalc((UC)data[T_I_TH-DEL],(UC)data[T_I_TL-DEL]);

        /// X
            _cur_Recv.x = HLDataCalc((UC)data[T_I_XH-DEL],(UC)data[T_I_XL-DEL],useUSB);
        /// Y
            _cur_Recv.y = HLDataCalc((UC)data[T_I_YH-DEL],(UC)data[T_I_YL-DEL],useUSB);
        /// Z
            _cur_Recv.z = HLDataCalc((UC)data[T_I_ZH-DEL],(UC)data[T_I_ZL-DEL],useUSB);
        /// A
            _cur_Recv.a = HLDataCalc((UC)data[T_I_AH-DEL],(UC)data[T_I_AL-DEL],useUSB);
        /// B
            _cur_Recv.b = HLDataCalc((UC)data[T_I_BH-DEL],(UC)data[T_I_BL-DEL],useUSB);
        /// C
            _cur_Recv.c = HLDataCalc((UC)data[T_I_CH-DEL],(UC)data[T_I_CL-DEL],useUSB);
        ///

#ifndef ZXX_DEBUG
    cout<<"急停:"<<_cur_Recv.criticalStop<< endl;
    cout<<"暂停/运行:"<<_cur_Recv.isPause<< endl;
    cout<<"使能:"<<_cur_Recv.enMotion<< endl;
    cout<<"收丝:"<<_cur_Recv.inSilk<< endl;
    cout<<"送丝:"<<_cur_Recv.outSilk<< endl;
    cout<<"运动类型:"<<_cur_Recv.motionType<< endl;
    cout<<"坐标类型:"<<_cur_Recv.coordinate<< endl;
    cout<<"Speed:"<<_cur_Recv.speed<< endl;

#endif
    UC ctr = _cur_Recv.enMotion ? 0x3 : 0x2;
    OpenLED(ctr);
#if 0   /// 按键响应
    RobotHelper::GetInstance()->CriticalStop(_cur_Recv.criticalStop);
    RobotHelper::GetInstance()->CriticalStop(_cur_Recv.isPause);
    RobotHelper::GetInstance()->OutSilk(_cur_Recv.outSilk);
    RobotHelper::GetInstance()->InSilk(_cur_Recv.inSilk);
#endif

    /// 刷新曲线，如果不希望影响性能，请屏蔽这里
    double tt[6] = {_cur_Recv.x,_cur_Recv.y,_cur_Recv.z,_cur_Recv.a,_cur_Recv.b,_cur_Recv.c};
    _send2Chart(tt);
    _send2Form(&_cur_Recv);

            /// 如果没有急停且正在使能按钮被按下，才允许移动
            if(!_cur_Recv.criticalStop && _cur_Recv.enMotion && !_cur_Recv.isPause){
                int _speed =round( 1.0 * _cur_Recv.speed * 100.00 / T_P_SPEED_MAX );
                _serialData(UC2ToPercent(_cur_Recv.x),UC2ToPercent(_cur_Recv.y),UC2ToPercent(_cur_Recv.z),_speed,_cur_Recv.motionType,_cur_Recv.coordinate);
            }else {

            }




    }else{
        //qWarning("错误的数据:");
//        QByteArray ARRAY;
//        foreach (UC d, data) {
//            ARRAY.push_back(d);
//        }
//        qDebug()<<ARRAY;
    }
}

int HandleHelper::OpenLED(UC ctr)
{
    QByteArray array;
    array.push_back((UC)0xaf);
    array.push_back((UC)0x20);
    array.push_back(ctr);
    array.push_back((UC)0x00);
    array.push_back((UC)0x00);
    array.push_back((UC)0x00);
    array.push_back((UC)0xf5);
    sendToDevice(array);
}

void HandleHelper::InstallHeartDance()
{
    std::thread([this](){
        while(1){
            Millsecond m1 = std::chrono::steady_clock::now() ;
            if( (recvTime - m1).count() > 3000){
                throw HD_HEART_ERROR;
                OpenLED(0x00);
            }
            std::this_thread::sleep_for( std::chrono::milliseconds(1000) ) ;
        }
    }).detach();
}


bool HandleHelper::CRCCheck(string data)
{
    if(data.size() < 20){
        return false;
    }
    UC2 crc = 0x0000;
    for(int i=1;i < T_P_DATA_LENTH - 1;i++){
        crc += (UC)data[i] ;
    }

    crc &=  T_P_CHECK;
    if(crc - (UC)data[T_P_DATA_LENTH-1]){
        return false;
    }
    return true;
}

/// 处理信号消息
void HandleHelper::handlMessage(std::string data)
{
    if(allow_handler && allow_send){
        auto th = new std::thread(&HandleHelper::DecodeData,this, data);
        th->detach();
    }else{
        throw HD_COMU_ERROR;
    }
}


/// 初始化通信模块
void HandleHelper::initCommunication()
{

    try{
        comType = (T_COM_TYPE)_comConf.get<int>("comType");
    }catch(...){
        throw HD_NO_CONF_ERROR;
    }
    switch (comType) {
    case T_RS_232:
        SerialHelper::GetInstance()->p()->OpenPort(_comConf.get<string>("RS232.name").c_str(),_comConf.get<int>("RS232.port"));
        break;
    case T_USB:
        USBHelper::GetInstance()->p()->OpenPort(_comConf.get<UC2>("USB.pid"),_comConf.get<UC2>("USB.vid"));
        break;
    case T_RS_422:
        SerialHelper::GetInstance()->p()->OpenPort(_comConf.get<string>("RS422.name").c_str(),_comConf.get<int>("RS422.port"));
        break;
    default:
        throw HD_NO_CONF_ERROR;
        break;
    }
    CreatService();
    ConnectDevice();
    InstallHeartDance();
}

/// 绑定信号事件
void HandleHelper::InstallMessage()
{
    SerialHelper::GetInstance()->p()->_sigUpdate.connect(boost::bind(&HandleHelper::handlMessage, this, _1));
    USBHelper::GetInstance()->p()->_sigUpdate.connect(boost::bind(&HandleHelper::handlMessage, this, _1));
}

/// 终止当前手柄线程
void HandleHelper::StopService()
{
    allow_handler = false;
    OpenLED(0x00);
    int ret = 0;
    if(comType == T_USB){
        ret = USBHelper::GetInstance()->p()->CloseDevice();
    }else{
        ret =SerialHelper::GetInstance()->p()->CloseSerial();
    }
    if(ret != 0){
        throw HD_CLOSE_ERROR;
    }
}



/// 连接通讯模块
bool HandleHelper::ConnectHandler()
{
    initCommunication();
    if(comType == T_USB){
        allow_handler = USBHelper::GetInstance()->p()->GetOpened();
    }else{
        allow_handler = SerialHelper::GetInstance()->p()->GetOpened();
    }

    return allow_handler;
}

/// 连接通讯模块并启动手柄事件循环
int HandleHelper::startListen()
{
    while (!ConnectHandler()) {
        static int count = 0;
        if(count > 10){
            throw HD_COMU_ERROR;
            return -1;
        }
        std::this_thread::sleep_for( std::chrono::milliseconds(1000) ) ;
        cout <<"\t >等待手柄连接完成! "<< "\t" << count <<endl;
        count++;
    }
    return HD_NO_ERROR;
}




/* 创建事件循环,用于监听某项事件 */
void HandleHelper::CreatService()
{
    if(comType == T_USB){
        _workService.reset(new std::thread([this](){
            while (USBHelper::GetInstance()->p()->ReadBuff()) {
                std::this_thread::sleep_for( std::chrono::milliseconds(100) ) ;
            }
        }));
    }else{
#if USE_ASYNC
        _workService.reset(new std::thread([this](){
            std::cout<<"\t 启用异步方式读取!"<<std::endl;
        }));

#elif
        _workService.re_workService.reset(new std::thread([this](){set(new std::thread([this](){
            long index = 0;
            static clock_t current_Clock = 0;
            static clock_t last_Clock = clock();;
            double TheTimes;
            while (SerialHelper::GetInstance()->p()->ReadBuff()) {
                current_Clock = clock();
                TheTimes=(double)((current_Clock - last_Clock)/CLOCKS_PER_SEC);
                std::cout<<TheTimes<<"\t"<<index++<<std::endl;

                std::this_thread::sleep_for( std::chrono::milliseconds(100) ) ;
                last_Clock = current_Clock;
            }
            std::cout<<"消息同步循环结束!"<<std::endl;
        }
        ));
#endif
    }



}
